//!Includes

#include "PBVScontrol.h"

//!Global Functions


namespace algorithms
{

/**
 * class PBVScontrol
 *
 *
 * Autor:David Bevilaqua
 *
 *
 * DESCRIPTION:
 *
 */

//!Constructors/Destructors

/**
 * Empty Constructor
 */
PBVScontrol::PBVScontrol ( )
{
	initAttributes();
}

/**
 * Empty Destructor
 */
PBVScontrol::~PBVScontrol ( )
{

}

//!Public Accessor Methods
void PBVScontrol::SetTargetVecotor(std::vector <algorithms::Target> TargetVector)
{
m_TargetVector=TargetVector;
}

int PBVScontrol::GetVelWheelLeft()
{
	return m_iVelWheelLeft;
}

int PBVScontrol::GetVelWheelRight()
{
	return m_iVelWheelRight;
}

int PBVScontrol::GetVelLinear()
{
	return m_iVelLinear;
}

int PBVScontrol::GetVelRot()
{
	return m_iVelRot;
}

bool PBVScontrol::StopRobot()
{

	return m_bStopRobot;
}

bool PBVScontrol::GetAchieveTarget()
{
return m_bAchieveTarget;
}

//!Public Methods

void PBVScontrol::LostTarget(bool lost)
{
m_bLost=lost;
}

void PBVScontrol::Execute()
{
	if(m_bLost)
	{
		SetVelLinear(m_dbKp*m_TargetVector.back().GetP()/10);
		SetVelAngular(m_dbKa*m_TargetVector.back().GetAlfa()+m_dbKb*m_TargetVector.back().GetBeta());
	}
	else
	{
		SetVelLinear(m_dbKp*m_TargetVector.back().GetP()/10);
		SetVelAngular(m_dbKa*m_TargetVector.back().GetAlfa()+m_dbKb*m_TargetVector.back().GetBeta());
	}
	//printf("m_iKp:%f,m_iKa:%f,m_iKb:%f\n",m_dbKp,m_dbKa,m_dbKb);
	//printf("GetP(): %d  GetAlfa(): %d: GetBetha:%d\n",m_TargetVector.back().GetP(),m_TargetVector.back().GetAlfa(),m_TargetVector.back().GetBeta());
	//printf("VelAngular: %f\n",m_dbKa*m_TargetVector.back().GetAlfa()+m_dbKb*m_TargetVector.back().GetBeta());
	//printf("VelLinear: %f\n",m_dbKp*m_TargetVector.back().GetP());
	//printf("m_iVelWheelLeft: %d  m_iVelWheelRight: %d\n",m_iVelWheelLeft,m_iVelWheelRight);
	printf("x: %d  y: %d,th: %d\n",m_TargetVector.back().GetPoseRobot().GetPose().x,m_TargetVector.back().GetPoseRobot().GetPose().y,m_TargetVector.back().GetPoseRobot().GetOrientation());
	if(m_TargetVector.back().GetP()>50||std::abs(m_TargetVector.back().GetAlfa())>10||std::abs(m_TargetVector.back().GetBeta())>10)
	{
		m_bStopRobot=false;
	}
	else
	{
		m_bAchieveTarget=true;
		m_bStopRobot=true;
	}
	m_TargetVector.clear();
}

//!Protected Methods



//!Private Methods
void PBVScontrol::SetVelLinear(int val)
{

	if(val>500)
	{
		val=500;
	}
	else if(val<-500)
	{
		val=-500;
	}
	m_iVelLinear=val;

	m_iVelWheelLeft=val;
	m_iVelWheelRight=val;
}

void PBVScontrol::SetVelAngular(int val)
{
	if(val>50)
	{
		val=50;
	}
	if(val<-50)
	{
		val=-50;
	}
	m_iVelRot=val;

	m_iVelWheelLeft=m_iVelWheelLeft-val;
	m_iVelWheelRight=m_iVelWheelRight+val;
}



/**
 * Starts all the attributes of
 * the PBVScontrol
 */
void PBVScontrol::initAttributes()
{
m_bAchieveTarget=false;
m_bStopRobot=false;
m_iVelWheelLeft=0;
m_iVelWheelRight=0;
m_dbKp=3;
m_dbKa=2;
m_dbKb=-1.5;
}


}
